import rclpy  
from rclpy.node import Node  
from geometry_msgs.msg import Twist  
import numpy as np  
from std_msgs.msg import Int32
from origincar_msg.msg import Data
import math
from math import atan2, sqrt, sin, cos
from ai_msgs.msg import PerceptionTargets
from rclpy.time import Time


# 任务三：停车点导航
class InertialNavigationNode3(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node_task3')  # 初始化节点名称

        self.last_log_time_navagation_3 = self.get_clock().now()
        
        
        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航   
        self.state = 0
        self.task = 0
        # 创建目标位置订阅，主要用于订阅目标位置,分别是x.y和角度
        self.subscription_robotpose = self.create_subscription(
            Data,  
            '/robotpose',
            self.robotpose_callback,
            2
        )
        
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.subscription_sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10) 
        
        #创建订阅者，订阅话题 hobot_dnn_detection 话题   
        self.subscription_fcos = self.create_subscription( 
            PerceptionTargets, 
            '/hobot_dnn_detection',  
            self.fcos_callback, 
            10)  
        
        #创建定时器发布cmd_vel
        self.timer = self.create_timer(
            timer_period_sec=1/30,  # 时间间隔（秒）
            callback=self.timer_callback  # 回调函数
        )

        qos = rclpy.qos.QoSProfile(
            history = rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
            depth = 10, 
            reliability = rclpy.qos.QoSReliabilityPolicy.RELIABLE,
            durability  = rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL,
            lifespan=rclpy.duration.Duration(seconds=1),  # 1 秒
        )
        
        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 1)  
  
  
        # 初始化状态变量用来储存位置
        self.angular =  0
        self.position = np.array([0.0, 0.0])  
        self.half_position = np.array([0.8, 1.6])
        self.target_position = np.array([0.5, 0.0])  # 目标位置     

        
        #锥桶的相对坐标在这里，分别是底边中点x,y,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        #停车点的相对坐标
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])

        self.jishiqi = 0
        self.t3_time = self.get_clock().now()
    
        self.vx = 1.6
        self.vx_3 = 0.9
        

        
        
    def fcos_callback(self, msg):
        if(self.task == 3):
            for num, target in enumerate(msg.targets):
                
                if target.rois:
                    
                    class_name = target.rois[0].type.strip()
                    # 获取第一个 ROI
                    roi = target.rois[0].rect

                    # 获取框参数
                    x_offset = roi.x_offset
                    y_offset = roi.y_offset
                    height = roi.height
                    width = roi.width
                                            
                    if class_name == 'p':
                        x = int(x_offset +  0.5 * width)
                        y = int(y_offset)
                         
                        self.park_relativ_position = x,y,width,height

                        self.state = 3
                        self.navigation_3()   
                                                                   
                        return
                                             
            self.state = 0    
        
    def timer_callback(self):

        if self.task == 3 :
            self.navigation_control()
        


    def robotpose_callback(self, msg):
        # 更新机器人的x轴位置坐标
        self.position[0] = msg.x
        # 更新机器人的y轴位置坐标
        self.position[1] = msg.y
        # 更新机器人的角度
        self.angular = msg.z
        
            
    def sign4return_callback(self,msg): #sign4return话题的回调函数，在foxglove上显示
    # 创建 Sign 消息实例
        if msg.data == 6:     
            self.task = 3        
            # 定位任务三目标点
            
            self.state = 0
            self.t3_time = self.get_clock().now()
            self.target_position[0] = 0.4
            self.target_position[1] = -0.4
              
        if msg.data == 5:
            #手动导航
            self.task = 0
            self.state = 0
            self.jishiqi = 0
            
            twist = Twist()
            twist.linear.x = 0.0
            twist.angular.z = 0.0
  
            self.cmd_pub.publish(twist)   
                      
            self.get_logger().info(f"推出任务3")
        return
    
    
    def navigation_control(self): #总导航控制逻辑
        dt = self.get_clock().now() - self.t3_time
        dt_sec = dt.nanoseconds / 1e9  # 转换为秒（float）

        if self.task == 3 :
            if self.state == 0 :
                self.navigation_0(self.target_position[0],self.target_position[1])
                
                                                          
                                

    def navigation_0(self,target_x,target_y): #state=0 无避障简单惯导
      
        
        # 计算目标点的角度
        dx = target_x - self.position[0]
        dy = target_y - self.position[1]
    
        target_angle_global = atan2(dy, dx)
        angle_diff = target_angle_global - self.angular
        angle_diff = atan2(sin(angle_diff), cos(angle_diff))
        twist = Twist()
        twist.linear.x = self.vx
        twist.angular.z = 2.5 * angle_diff
  
        
        self.cmd_pub.publish(twist)  
        self.get_logger().info('self.position[0]{:.3f}self.position[1]{:.3f}self.angular{:.3f}'.format(self.position[0],self.position[1],self.angular))
        self.get_logger().info('target_x{:.3f}target_y{:.3f}target_angle={:.3f}'.format(target_x,target_y,angle_diff))
        self.get_logger().info('self.state = {:.3f}'.format(self.state))


                      


    def navigation_3(self): #state=3，cv识别到停车点，进行导航，
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - 0.004 * (self.park_relativ_position[0] - 320 ) # 设置角速度
       
        self.get_logger().info(f"识别park点")  
        self.get_logger().info('self.park_relativ_position[0]{:.3f}'.format(self.park_relativ_position[0]))

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

    

def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation_task3 = InertialNavigationNode3()  #
    rclpy.spin(navigation_task3)
    navigation_task3.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数

